import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.LightSensor;
import lejos.nxt.TouchSensor;
import lejos.robotics.RegulatedMotor;
import lejos.util.Delay;

public class Challenge02
{
    //public int direction = 1;
    //i'm taking a wild guess that speed = rotations per second
    private int speed;
    private int speedA;
    private int speedB;
    
    //rotations of each wheel
    private int rot;
    private int rotA;
    private int rotB;
    
    private double pi = 3.14159265358;
    //diameters
    // 2 is the outside wheel,
    // 10 is the wheelbase to the nearest int (cm)
    
    private int di1;
    private int di2 = di1 + 10;
    
    // circumferences
    // for smooth corners.
    private int ci1;
    private int ci2;
    
    //the ratio ( it should make sense )
    private double ratio;

    private boolean done;
    
    private final SensorPort ultrasonicPort;
    private final UltrasonicSensor ultrasonicSensor;
    private boolean sonicIsOn = true;
    
    private final SensorPort lightPort;
    private final LightSensor lightSensor;
    private boolean lightIsOn = true;
    
    private final SensorPort touchPort;
    private TouchSensor touchSensor;
    private boolean touchIsOn = true;
    private int touchMode = 1;
    
    public static void main(String[] args)
    {
   	 final Challenge02 robot;
   	 robot = new Challenge02();
   	 robot.run();
    }
    
    public Challenge02()
    {
   	 ultrasonicPort = SensorPort.S1;
   	 ultrasonicSensor = new UltrasonicSensor(ultrasonicPort);
   	 
   	 touchPort = SensorPort.S2;
   	 touchSensor = new TouchSensor(touchPort);
   	 touchPort.addSensorPortListener(new TouchListener());
   	 
   	 lightPort = SensorPort.S4;
   	 lightSensor = new LightSensor(lightPort);
   	 lightSensor.setFloodlight(true);
    }
    
    public void run()
    {
   	 // Setting Defaults
   	 System.out.println("Setting speed");
   	 int distance;
   	 Motor.B.setSpeed(360);
   	 Motor.A.setSpeed(360);
   	 
   	 // Find to Wall
   	 System.out.println("Finding Wall");
   	 int position = 0;
   	 int closestPing = ultrasonicSensor.getDistance();
   	 for(int i = 0; i < 36; ++i)
   	 {
   		 Motor.B.rotate( 17, true);
   		 Motor.A.rotate(-17);
   		 
   		 Delay.msDelay(100);
   		 int tempPing = ultrasonicSensor.getDistance();
   		 if(tempPing <= closestPing)
   		 {
   			 closestPing = tempPing;
   			 position = i;
   		 }
   	 }
   	 
   	 System.out.println("The Closest position is: " + position + " at " + closestPing + "cm");
   	 
   	 // These values need to be tuned
   	 System.out.println("Rotating for alignment");
   	 Motor.B.rotate(-17 * (36 - position), true);
   	 Motor.A.rotate( 17 * (36 - position));
   	 
   	 // Move to Wall
   	 System.out.println("Moving to wall");
   	 moveForwards();
   	 waitForDistance(10);
   	 
   	 // get parallel to wall
   	 System.out.println("Aligning with wall");
   	 go(150);
   	 reverse(50);
   	 turnLeft90();
   	 while(true)
   	 {
   		 // Move Along Wall
   		 System.out.println("Moving along wall");
   		 moveForwards();
   		 Delay.msDelay(1000);
   		 
   		 System.out.println("Waiting for door");
   		 
   		 while(waitForLight(1023) < 430)
   		 {}
   		 //Try Door
   		 System.out.println("Turning into door");
   		 reverse(5);
   		 
   		 turn(-1);
   		 System.out.println("Running into door");
   		 go(180);
   		 reverse(45)
   		 System.out.println("Checking door still standing");
   		 int door;
   		 door = ultrasonicSensor.getDistance();
   		 if (door > 20)
   		 {
   		 System.out.println("Exit door");
   		 go(180);
   		 }
   		 System.out.println("Leaving closed door");
   		 reverse(45);
   		 turn(1);
   		 go(25); // so that he wont try same door over and over?
   		 //is in a corner?
   		 int wall;
   		 wall = ultrasonicSensor.getDistance();
   		 if (wall < 20)
   		 {
   		 moveForwards();
   		 waitForDistance(10);
   		 turnLeft90();
   		 }
   	 }
    }
    
    private void moveForwards()
    {
   	 System.out.println("Moving Forwards...");
   	 touchIsOn = true;
   	 Motor.B.forward();
   	 Motor.A.forward();
    }
    
    /**
     *  direction 1 Left ||| -1 Right
     */
    
    
    public void turnLeft90()
    {
   	 if(done == true)
   	 {
   		 done = false;    
   	 }
   		 Motor.B.rotate(158 * 1, true);
   		 Motor.A.rotate(-158 * 1);
    }
    
    public void turnRight90()
    {
   	 if(done == true)
   	 {
   		 done = false;
   	 }
   		 Motor.B.rotate(-158 * 1, true);
   		 Motor.A.rotate(158 * 1);
    }
    
    private void pivotRight()
    {
   	 System.out.println("Turning Right 90 degrees...");
   	 Motor.B.rotate(154);
    }
    private void pivotLeft()
    {
   	 System.out.println("Turning Left 90 degrees...");
   	 Motor.A.rotate(154);
    }
    private void turn(int x)
    {
   	 System.out.println("turning");
   	 Motor.B.rotate(154 * x, true);
   	 Motor.A.rotate(-154 * x);
    }
    
    private void go(int x)
    {
   	 System.out.println("going...");
   	 {    
   		 Motor.B.rotate(x, true);
   		 Motor.A.rotate(x);    
   	 }
    }    
    private void reverse(int x)
    {
   	 System.out.println("Reversing...");
   	 x = x * -1;
   	 {
   		 Motor.B.rotate(x, true);
   		 Motor.A.rotate(x);    
   	 }
    }    
    
    private class TouchListener implements SensorPortListener
    {
   	 public void stateChanged(final SensorPort port,
   	 final int oldValue, final int newValue)
   	 {
   		 if(touchSensor.isPressed())
   		 {
   			 stop();
   		 }
   	 }
    }
    
    private void waitForStop()
    {
   	 System.out.println("Waiting for Touch listener");
   	 while(!(done))
   	 {
   		 try
   		 {
   			 synchronized(this)
   			 {
   				 wait(1);
   			 }
   		 }
   		 catch(final InterruptedException ex)
   		 {
   		 }
   	 }
   	 done = false;
    }
    
    private void stop()
    {
   	 synchronized(this)
   	 {
   		 System.out.println("Trigger Touch Listener");
   		 touchIsOn = false;
   		 done = true;
   		 
   		 reverse(77);
   		 turn(1);
   	 }
    }
    
    private int waitForDistance(final int max)
    {
   	 int distance;
   	 
   	 do
   	 {
   		 try
   		 {
   			 // pause 100 ms between reads...
   			 // this is a LeJOS issue
   			 // later versions of LeJOS won't require this
   			 Thread.sleep(100);
   		 }
   		 catch (InterruptedException ex)
   		 {
   		 }
   		 
   		 distance = ultrasonicSensor.getDistance();
   	 }
   	 
   	 while(distance > max);
   	 return (distance);
    }
    
    private int waitForLight(final int max)
    {
   	 int light;
   	 
   		 light = lightSensor.readNormalizedValue();
   	 
   	 
   	 return (light);
    }
}
